#PACKET STRUCTURE
#'v' 'e' 'x'
#RX[3] = checksum
#RX[4] = 'S'
#RX[5] = 34
#RX[6] = motor_mode
#RX[7] = commutation_mode
#RX[8] = loop_mode
#RX[9] = dead_time
#RX[10]/RX[11] = motor_limit
#RX[12]/RX[13] = motor_phase_counts
#RX[14]/RX[15] = Velocity Scalar
#RX[16]/RX[17]/RX[18]/RX[19] = Accelleration
#RX[20]/RX[21] = Velocity Loop Gain Kp
#RX[22]/RX[23] = Velocity Loop Gain Ki
#RX[24]/RX[25] = Velocity Loop Gain Limit
#RX[26]/RX[27] = Current Loop Gain Kp
#RX[28]/RX[29] = Current Loop Gain Ki
#RX[30]/RX[31] = Current Loop Gain Limit
#RX[32] = Temp Max
#RX[33] = Motor Command Source

#RX[34] = 'u'


#For Enable
#RX[4] = 'E'
#RX[5] = 7
#RX[6] = state
#RX[7] = 'u'

#For Motor Command Set
#RX[4] = 'C'
#Rx[5] = 8
#Rx[6]/RX[7] = Motor/Velocity Command
#RX[8] = 'u'

from PyQt4 import QtCore, QtGui
import serial
import time
from array import array

serial_port = "/dev/ttyUSB0"
baud_rate = 115200
motor_command = 25
motor_mode = 0
motor_commutation_mode = 0
motor_loop_mode = 0
motor_dead_time = 55
motor_armed = 0
motor_condition_mask1 = 0
motor_limit = 10000
motor_pwm_sense = 0
motor_signal_sense = 0
motor_condition_mask2 = 0
motor_phase_counts = 100



#Setting Command Length
settings_command_length = 34

self_clock = 0;

actual_dead_times = [0]*56
actual_dead_times[0] = "0"
actual_dead_times[1] = "25ns"
actual_dead_times[2] = "50ns"
actual_dead_times[3] = "75ns"
actual_dead_times[4] = "100ns"
actual_dead_times[5] = "125ns"
actual_dead_times[6] = "150ns"
actual_dead_times[7] = "175ns"
actual_dead_times[8] = "200ns"
actual_dead_times[9] = "225ns"
actual_dead_times[10] = "250ns"
actual_dead_times[11] = "275ns"
actual_dead_times[12] = "300ns"
actual_dead_times[13] = "325ns"
actual_dead_times[14] = "350ns"
actual_dead_times[15] = "375ns"
actual_dead_times[16] = "400ns"
actual_dead_times[17] = "450ns"
actual_dead_times[18] = "500ns"
actual_dead_times[19] = "550ns"
actual_dead_times[20] = "600ns"
actual_dead_times[21] = "650ns"
actual_dead_times[22] = "700ns"
actual_dead_times[23] = "750ns"
actual_dead_times[24] = "800ns"
actual_dead_times[25] = "900ns"
actual_dead_times[26] = "1us"
actual_dead_times[27] = "1.1us"
actual_dead_times[28] = "1.2us"
actual_dead_times[29] = "1.3us"
actual_dead_times[30] = "1.4us"
actual_dead_times[31] = "1.5us"
actual_dead_times[32] = "1.6us"
actual_dead_times[33] = "1.8us"
actual_dead_times[34] = "2us"
actual_dead_times[35] = "2.2us"
actual_dead_times[36] = "2.4us"
actual_dead_times[37] = "2.6us"
actual_dead_times[38] = "2.8us"
actual_dead_times[39] = "3us"
actual_dead_times[40] = "3.2us"
actual_dead_times[41] = "3.6us"
actual_dead_times[42] = "4us"
actual_dead_times[43] = "4.4us"
actual_dead_times[44] = "4.8us"
actual_dead_times[45] = "5.2us"
actual_dead_times[46] = "5.6us"
actual_dead_times[47] = "6us"
actual_dead_times[48] = "6.4us"
actual_dead_times[49] = "7.2us"
actual_dead_times[50] = "8us"
actual_dead_times[51] = "8.8us"
actual_dead_times[52] = "9.6us"
actual_dead_times[53] = "10.4us"
actual_dead_times[54] = "11.2us"
actual_dead_times[55] = "12us"


try:
    ser = serial.Serial(serial_port, baud_rate, timeout=1)
    print("Serial at %s") % (serial_port)
except:
    print("Serial Unavailable at %s!") % (serial_port)

def twos_comp(val, bits):
    #"""compute the 2's compliment of int value val"""
    if( (val&(1<<(bits-1))) != 0 ):
        val = val - (1<<bits)
    return val

class ThunderboxGUI(QtGui.QWidget):
    def send_data(self):
        try:
            global settings_command_length
            check_sum = 0
            check_sum_test = ord('v') + ord('e') + ord('x') + ord('u') + ord('S') + settings_command_length
            #motor_mode = int(self.motor_mode_line.text())
            if (self.motor_mode_check_box.checkState() == 2):
                motor_mode = 1
            else:
                motor_mode = 0

            check_sum_test += motor_mode
            #motor_commutation_mode = int(self.motor_commutation_mode_line.text())

            if (self.motor_commutation_sine_check_box.checkState() == 2):
                motor_commutation_mode = 0
            elif (self.motor_commutation_hall_check_box.checkState() == 2):
                motor_commutation_mode = 1
            elif (self.motor_commutation_sine_foc_check_box.checkState() == 2):
                motor_commutation_mode = 2
            elif (self.motor_commutation_hall_foc_check_box.checkState() == 2):
                motor_commutation_mode = 3
            else:
                motor_commutation_mode = 1

            check_sum_test += motor_commutation_mode

            motor_command_source = 2
            if (self.motor_command_source_spi_check_box.checkState() == 2):
                motor_command_source = 1
            elif (self.motor_command_source_profile_check_box.checkState() == 2):
                motor_command_source = 2

            check_sum_test += motor_command_source

            motor_loop_mode = 0
            if (self.loop_mode_current_check_box.checkState() == 2):
                motor_loop_mode += 1
            if (self.loop_mode_velocity_check_box.checkState() == 2):
                motor_loop_mode += 2
            if (self.loop_mode_velocity_integrate_check_box.checkState() == 2):
                motor_loop_mode += 4

            check_sum_test += motor_loop_mode
            motor_dead_time = int(self.motor_dead_time_line.text())
            check_sum_test += motor_dead_time
            motor_limit = int(self.motor_limit_line.text())
            check_sum_test  += (motor_limit >> 8) + motor_limit
            motor_phase_counts= int(self.motor_phase_counts_line.text())
            check_sum_test += (motor_phase_counts >> 8) + motor_phase_counts
            motor_velocity_scalar = int(self.motor_velocity_scalar_line.text())
            check_sum_test += (motor_velocity_scalar >> 8) + motor_velocity_scalar
            motor_acceleration = int(float(self.motor_acceleration_line.text()) * (2**16))
            check_sum_test += (motor_acceleration >> 24) + (motor_acceleration >> 16) +  (motor_acceleration >> 8) + motor_acceleration
            motor_velocity_gain_kp = int(float(self.motor_velocity_loop_gain_kp_line.text()) * 64)
            check_sum_test += (motor_velocity_gain_kp >> 8) + motor_velocity_gain_kp
            motor_velocity_gain_ki = int(float(self.motor_velocity_loop_gain_ki_line.text()) * 256)
            check_sum_test += (motor_velocity_gain_ki >> 8) + motor_velocity_gain_ki
            motor_velocity_gain_limit = int(self.motor_velocity_loop_gain_limit_line.text())
            check_sum_test += (motor_velocity_gain_limit >> 8) + motor_velocity_gain_limit


            motor_current_gain_kp = int(float(self.motor_current_loop_gain_kp_line.text()) * 64)
            check_sum_test += (motor_current_gain_kp >> 24) + (motor_current_gain_kp >> 16) + (motor_current_gain_kp >> 8) + motor_current_gain_kp
            motor_current_gain_ki = int(float(self.motor_current_loop_gain_ki_line.text()) * 256)
            check_sum_test += (motor_current_gain_ki >> 24) + (motor_current_gain_ki >> 16) + (motor_current_gain_ki >> 8) + motor_current_gain_ki
            motor_current_gain_limit = int(self.motor_current_loop_gain_limit_line.text())
            check_sum_test += (motor_current_gain_limit >> 8) + motor_current_gain_limit

            motor_temp_max = int(self.motor_temp_max_line.text()) + 55
            check_sum_test += motor_temp_max


            check_sum_test = check_sum_test & 0x00ff
            check_sum_test = 256 - check_sum_test

            ser.write('v' + 'e' + 'x')
	    ser.write(chr(check_sum_test))
            ser.write('S' + chr(settings_command_length))
            ser.write(chr(motor_mode))
	    ser.write(chr(motor_commutation_mode))
	    ser.write(chr(motor_loop_mode))
            ser.write(chr(motor_dead_time))
            ser.write(chr((motor_limit >> 8) & 0xff) + chr(motor_limit & 0xff))
            ser.write(chr((motor_phase_counts >> 8) & 0xff) + chr(motor_phase_counts & 0xff))
            ser.write(chr((motor_velocity_scalar >> 8) & 0xff) + chr(motor_velocity_scalar & 0xff))
            ser.write(chr((motor_acceleration >> 24) & 0xff) + chr((motor_acceleration >> 16) & 0xff) + chr((motor_acceleration >> 8) & 0xff) + chr(motor_acceleration & 0xff))
            ser.write(chr((motor_velocity_gain_kp >> 8) & 0xff) + chr(motor_velocity_gain_kp & 0xff))
            ser.write(chr((motor_velocity_gain_ki >> 8) & 0xff) + chr(motor_velocity_gain_ki & 0xff))
            ser.write(chr((motor_velocity_gain_limit >> 8) & 0xff) + chr(motor_velocity_gain_limit & 0xff))

            ser.write(chr((motor_current_gain_kp >> 8) & 0xff) + chr(motor_current_gain_kp & 0xff))
            ser.write(chr((motor_current_gain_ki >> 8) & 0xff) + chr(motor_current_gain_ki & 0xff))
            ser.write(chr((motor_current_gain_limit >> 8) & 0xff) + chr(motor_current_gain_limit & 0xff))
            ser.write(chr(motor_temp_max))
            ser.write(chr(motor_command_source))

            ser.write('u')
            self.get_status()
        except:
            print "send_data failed"
    def read_data(self):
    #    try:
            ser.write('give')
            time.sleep(1)
            read_counter = 0
            check_counter = 0
            RX = [0]*40
            while ser.inWaiting() != 0:
                RX[read_counter] = ser.read(1)
                if (RX[read_counter] == 'v') and (read_counter == 0):
                    read_counter = 1
                    check_counter = check_counter + RX[read_counter]
                elif (RX[read_counter] == 'e') and (read_counter == 1):
                    read_counter = 2
                    check_counter = check_counter + RX[read_counter]

                elif (RX[read_counter] == 'x') and (read_counter == 2):
                    read_counter = 3
                    check_counter = check_counter + RX[read_counter]

                elif (read_counter == 3):
                    read_counter = 4
                    check_counter = check_counter + RX[read_counter]

                elif ((read_counter > 3) and (read_counter < 32)):
                    read_counter = read_counter + 1
                    check_counter = check_counter + RX[read_counter]

                elif ((RX[read_counter] == 'u') and (read_counter == 32)): 
                    read_counter = 33
                    check_counter = check_counter + RX[read_counter]
                    if ((check_counter & 0xff) == 0):
                        if (ord(RX[4]) == 1):
#                            self.motor_mode_check_box.setCheckState (self, Qt.Checked)
                            self.motor_mode_check_box.setChecked(True)

                        else:
#                             self.motor_mode_check_box.setCheckState (self, Qt.Unchecked)
                             self.motor_mode_check_box.setChecked(False)

                        
                        if(ord(RX[31]) == 1):
                             self.motor_command_source_spi_check_box.setChecked(True)
                             self.stateChanged_spi(2)
                        elif(ord(RX[31]) == 2):
                             self.motor_command_source_profile_check_box.setChecked(True)
                             self.stateChanged_profile(2)

                        if(ord(RX[5]) == 0):
                            self.motor_commutation_sine_check_box.setChecked(True)
                            self.stateChanged_sine_check(2)
                        elif(ord(RX[5]) == 1):
                            self.motor_commutation_hall_check_box.setChecked(True)
                            self.stateChanged_hall_check(2)

                        elif(ord(RX[5]) == 2):
                            self.motor_commutation_sine_foc_check_box.setChecked(True)
                            self.stateChanged_sine_foc_check(2)

                        elif(ord(RX[5]) == 3):
                            self.motor_commutation_hall_foc_check_box.setChecked(True)
                            self.stateChanged_hall_foc_check(2)


                        if(ord(RX[6]) & 1):
                            self.loop_mode_current_check_box.setChecked(True)
                        else:
                            self.loop_mode_current_check_box.setChecked(False)
                        if(ord(RX[6]) & 2):
                            self.loop_mode_velocity_check_box.setChecked(True)
                        else:
                            self.loop_mode_velocity_check_box.setChecked(False)
                        if(ord(RX[6]) & 4):
                            self.loop_mode_velocity_integrate_check_box.setChecked(True)
                        else:
                            self.loop_mode_velocity_integrate_check_box.setChecked(False)

                        motor_dead_time_buf = "%d" % (ord(RX[7]))
                        self.motor_dead_time_line.setText(motor_dead_time_buf)
                        self.actual_motor_dead_time_line.setText(actual_dead_times[motor_dead_time])
                        motor_limit_buf = "%d" % ((ord(RX[8]) << 8) | ord(RX[9]))
                        self.motor_limit_line.setText(motor_limit_buf)
                        motor_phase_counts_buf = "%d" % ((ord(RX[10]) << 8) | ord(RX[11]))
                        self.motor_phase_counts_line.setText(motor_phase_counts_buf)

                        motor_velocity_scalar_buf = "%d" % ((ord(RX[12]) << 8) | ord(RX[13]))
                        self.motor_velocity_scalar_line.setText(motor_velocity_scalar_buf)

                        motor_acceleration_buf = "%f" % ((float)((ord(RX[14]) << 24) | (ord(RX[15]) << 16) | (ord(RX[16]) << 8) | ord(RX[17]))/(2**16))
                        self.motor_acceleration_line.setText(motor_acceleration_buf)

                        motor_velocity_loop_gain_kp_buf = "%f" % ((float)((ord(RX[18]) << 8) | ord(RX[19]))/64)
                        self.motor_velocity_loop_gain_kp_line.setText(motor_velocity_loop_gain_kp_buf)

                        motor_velocity_loop_gain_ki_buf = "%f" % ((float)((ord(RX[20]) << 8) | ord(RX[21]))/256)
                        self.motor_velocity_loop_gain_ki_line.setText(motor_velocity_loop_gain_ki_buf)

                        motor_velocity_loop_gain_limit_buf = "%d" % ((ord(RX[22]) << 8) | ord(RX[23]))
                        self.motor_velocity_loop_gain_limit_line.setText(motor_velocity_loop_gain_limit_buf)


                        motor_current_loop_gain_kp_buf = "%f" % ((float)((ord(RX[24])<<8) | ord(RX[25]))/64)
                        self.motor_current_loop_gain_kp_line.setText(motor_current_loop_gain_kp_buf)

                        motor_current_loop_gain_ki_buf = "%f" % ((float)((ord(RX[26]) << 8) | ord(RX[27]))/256)
                        self.motor_current_loop_gain_ki_line.setText(motor_current_loop_gain_ki_buf)

                        motor_current_loop_gain_limit_buf = "%d" % ((ord(RX[28]) << 8) | ord(RX[29]))
                        self.motor_current_loop_gain_limit_line.setText(motor_current_loop_gain_limit_buf)

                        motor_temp_max_buf = "%d" % (ord(RX[30]) - 55)
                        self.motor_temp_max_line.setText(motor_temp_max_buf) 
                        self.motor_status_label.setText("Read Succeeded")

                else:
                    self.motor_status_label.setText("Read Failed")
            if(read_counter < 29):
                 self.motor_status_label.setText("Read Failed")
               
 
 
    #    except:
    #        print "read_data failed"
    #        return 0


    def get_status(self):
        time.sleep(.1)
        read_counter = 0
        check_counter = 0
        RX = [0]*8
        while ser.inWaiting() != 0:
              RX[read_counter] = ser.read(1)
              if(read_counter < 5):
                  check_counter += ord(RX[read_counter])
                  read_counter += 1
        #try:     
        if(((check_counter & 0xff) == 0) and (read_counter > 0)):
                 if(ord(RX[0]) == 0):
                      self.motor_status_label.setText("Acknowledged")
                 elif(ord(RX[0]) & 1):
                      self.motor_status_label.setText("Estopped")
                 motor_vbus_buf = "Voltage Bus: %f" % ((3.3*((ord(RX[2]) << 8) | (ord(RX[3]) & 0xff))/(65535)) / .03861)
                 self.motor_vbus_label.setText(motor_vbus_buf)
                 temp_buf = "Temperature: %dC" % (ord(RX[4]))
                 self.temp_label.setText(temp_buf)

        else:
                 self.motor_status_label.setText("Bad Checksum")
                 #print check_counter
        #except:
        #    self.motor_status_label.setText("Not Acknowledged")
       

    def enable(self):

        global motor_armed
        check_sum = ord('v') + ord('e') + ord('x') + ord('u') + ord('E') + 7
        if(motor_armed == 1):
            motor_armed = 0
            self.enable_button.setText("&Enable")
        else:
            motor_armed = 1
            self.enable_button.setText("&Disable")

        check_sum = check_sum + motor_armed
        check_sum = 256 - (check_sum & 0xff)
        ser.write('vex' + chr(check_sum) + 'E' + chr(7) + chr(motor_armed) + 'u')
        self.get_status()

    def demand_acknowledge(self):

        check_sum = ord('v') + ord('e') + ord('x') + ord('u') + ord('A') + 6
        check_sum = 256 - (check_sum & 0xff)
        ser.write('vex' + chr(check_sum) + 'A' + chr(6) + 'u')
        self.get_status()

    def set_offset(self):

        check_sum = ord('v') + ord('e') + ord('x') + ord('u') + ord('D') + 6
        check_sum = 256 - (check_sum & 0xff)
        ser.write('vex' + chr(check_sum) + 'D' + chr(6) + 'u')
        self.get_status()


    def get_event_status(self):
        check_sum = ord('v') + ord('e') + ord('x') + ord('u') + ord('z') + 6
        check_sum = 256 - (check_sum & 0xff)
        ser.write('vex' + chr(check_sum) + 'z' + chr(6) + 'u')
        time.sleep(.1)
        read_counter = 0
        check_counter = 0
        RX = [0]*8
        while ser.inWaiting() != 0:
              RX[read_counter] = ser.read(1)
              if(read_counter < 4):
                  check_counter += ord(RX[read_counter])
                  read_counter += 1
        #try:     
        if(((check_counter & 0xff) == 0) and (read_counter > 0)):
                 if(ord(RX[0]) == 0):
                      self.motor_status_label.setText("Acknowledged")
                 elif(ord(RX[0]) & 1):
                      self.motor_status_label.setText("Estopped")
                 event_status_buf = "Event Status: %i" % ((ord(RX[2]) << 8) | (ord(RX[3]) & 0xff))
                 self.event_status_label.setText(event_status_buf)

        else:
                 self.motor_status_label.setText("Bad Checksum %i" % read_counter)

    def get_current(self):
        check_sum = ord('v') + ord('e') + ord('x') + ord('u') + ord('i') + 6
        check_sum = 256 - (check_sum & 0xff)
        ser.write('vex' + chr(check_sum) + 'i' + chr(6) + 'u')
        time.sleep(.1)
        read_counter = 0
        check_counter = 0
        RX = [0]*8
        while ser.inWaiting() != 0:
              RX[read_counter] = ser.read(1)
              if(read_counter < 6):
                  check_counter += ord(RX[read_counter])
                  read_counter += 1
        #try:     
        if(((check_counter & 0xff) == 0) and (read_counter > 0)):
                 if(ord(RX[0]) == 0):
                      self.motor_status_label.setText("Acknowledged")
                 elif(ord(RX[0]) & 1):
                      self.motor_status_label.setText("Estopped")
                 current_buf = "Current A: %i" % (twos_comp((ord(RX[2]) << 8) | (ord(RX[3]) & 0xff), 16))
                 self.motor_current_A_label.setText(current_buf)
                 current_buf = "Current B: %i" % (twos_comp((ord(RX[4]) << 8) | (ord(RX[5]) & 0xff), 16))
                 self.motor_current_B_label.setText(current_buf)



        else:
                 self.motor_status_label.setText("Bad Checksum %i" % read_counter)
    def get_error(self):
        check_sum = ord('v') + ord('e') + ord('x') + ord('u') + ord('d') + 6
        check_sum = 256 - (check_sum & 0xff)
        ser.write('vex' + chr(check_sum) + 'd' + chr(6) + 'u')
        time.sleep(.1)
        read_counter = 0
        check_counter = 0
        RX = [0]*8
        while ser.inWaiting() != 0:
              RX[read_counter] = ser.read(1)
              if(read_counter < 6):
                  check_counter += ord(RX[read_counter])
                  read_counter += 1
        #try:     
        if(((check_counter & 0xff) == 0) and (read_counter > 0)):
                 if(ord(RX[0]) == 0):
                      self.motor_status_label.setText("Acknowledged")
                 elif(ord(RX[0]) & 1):
                      self.motor_status_label.setText("Estopped")
                 current_error_buf = "Current A Error: %i" % (twos_comp((ord(RX[2]) << 8) | (ord(RX[3]) & 0xff), 16))
                 self.current_A_error_label.setText(current_error_buf)
                 current_error_buf = "Current B Error: %i" % (twos_comp((ord(RX[4]) << 8) | (ord(RX[5]) & 0xff), 16))
                 self.current_B_error_label.setText(current_error_buf)


        else:
                 self.motor_status_label.setText("Bad Checksum %i" % read_counter)



    def emergency_stop(self):

        check_sum = ord('v') + ord('e') + ord('x') + ord('u') + ord('K') + 6
        check_sum = 256 - (check_sum & 0xff)
        ser.write('vex' + chr(check_sum) + 'K' + chr(6) + 'u')
        self.get_status()




    def look_out(self):

        global self_clock
        check_sum = ord('v') + ord('e') + ord('x') + ord('u') + ord('L') + 7
        if(self_clock == 1):
            self_clock = 0
            self.look_out_button.setText("Enable Auto-Update")
        else:
            self_clock = 1
            self.look_out_button.setText("Disable Auto-Update")

        check_sum = check_sum + self_clock
        check_sum = 256 - (check_sum & 0xff)
        ser.write('vex' + chr(check_sum) + 'L' + chr(7) + chr(self_clock) + 'u')
        self.get_status()




    def command(self):
        check_sum = ord('v') + ord('e') + ord('x') + ord('u') + ord('C') + 8
        motor_command = int(self.motor_command_line.text())
        check_sum += (motor_command >> 8) + motor_command
        check_sum = 256 - (check_sum & 0xff)
        ser.write('vex' + chr(check_sum) + 'C' + chr(8) + chr((motor_command >> 8) & 0xff) + chr(motor_command & 0x00ff) + 'u')
        self.get_status()

    def get_command(self):
        check_sum = ord('v') + ord('e') + ord('x') + ord('u') + ord('c') + 6
        check_sum = 256 - (check_sum & 0xff)
        ser.write('vex' + chr(check_sum) + 'c' + chr(6) + 'u')
        time.sleep(.1)
        read_counter = 0
        check_counter = 0
        RX = [0]*8
        while ser.inWaiting() != 0:
              RX[read_counter] = ser.read(1)
              if(read_counter < 4):
                  check_counter += ord(RX[read_counter])
                  read_counter += 1

        if(((check_counter & 0xff) == 0) and (read_counter > 0)):
                 motor_command_buf = "%d" % (twos_comp(((ord(RX[2]) << 8) | (ord(RX[3]) & 0xff)), 16))
                 self.motor_command_line.setText(motor_command_buf)

        else:
                 self.motor_status_label.setText("Bad Checksum")

        

    def spi_command(self):
        check_sum = ord('v') + ord('e') + ord('x') + ord('u') + ord('X') + 8
        motor_spi_command = int(self.motor_spi_command_line.text())
        check_sum += (motor_spi_command >> 8) + motor_spi_command
        check_sum = 256 - (check_sum & 0xff)
        ser.write('vex' + chr(check_sum) + 'X' + chr(8) + chr((motor_spi_command >> 8) & 0xff) + chr(motor_spi_command & 0xff) + 'u')
        self.get_status()


    def get_spi_command(self):
        check_sum = ord('v') + ord('e') + ord('x') + ord('u') + ord('x') + 6
        check_sum = 256 - (check_sum & 0xff)
        ser.write('vex' + chr(check_sum) + 'x' + chr(6) + 'u')
        time.sleep(.1)
        read_counter = 0
        check_counter = 0
        RX = [0]*8
        while ser.inWaiting() != 0:
              RX[read_counter] = ser.read(1)
              if(read_counter < 6):
                  check_counter += ord(RX[read_counter])
                  read_counter += 1
        #try:     
        if(((check_counter & 0xff) == 0) and (read_counter > 0)):
                 motor_spi_command_buf = "%d" % (twos_comp((ord(RX[2]) << 8) | (ord(RX[3]) & 0xff), 16))
                 self.motor_spi_command_line.setText(motor_spi_command_buf)

        else:
                 self.motor_status_label.setText("Bad Checksum")



    def velocity(self):
        check_sum = ord('v') + ord('e') + ord('x') + ord('u') + ord('V') + 10
        motor_velocity = int(float(self.motor_velocity_line.text()) * (2**16))
        check_sum += (motor_velocity >> 24) + (motor_velocity >> 16) + (motor_velocity >> 8) + motor_velocity
        check_sum = 256 - (check_sum & 0xff)
        ser.write('vex' + chr(check_sum) + 'V' + chr(10) + chr((motor_velocity >> 24) & 0xff) + chr((motor_velocity >> 16) & 0xff) + chr((motor_velocity >> 8) & 0xff) + chr(motor_velocity & 0xff) + 'u')
        self.get_status()

    def get_velocity(self):
        check_sum = ord('v') + ord('e') + ord('x') + ord('u') + ord('v') + 6
        check_sum = 256 - (check_sum & 0xff)
        ser.write('vex' + chr(check_sum) + 'v' + chr(6) + 'u')
        time.sleep(.1)
        read_counter = 0
        check_counter = 0
        RX = [0]*8
        while ser.inWaiting() != 0:
              RX[read_counter] = ser.read(1)
              if(read_counter < 6):
                  check_counter += ord(RX[read_counter])
                  read_counter += 1
        #try:     
        if(((check_counter & 0xff) == 0) and (read_counter > 0)):
                 motor_velocity_buf = "%f" % ((float)(twos_comp(((ord(RX[2]) << 24) | (ord(RX[3]) << 16) | (ord(RX[4]) << 8) | (ord(RX[5]) & 0xff)), 32))/(2**16))
                 self.motor_velocity_line.setText(motor_velocity_buf)

        else:
                 print check_counter
                 self.motor_status_label.setText("Bad Checksum")


       





    def changeValue_dead_time(self, value):
        dead_time = (value * 56)/100
        motor_dead_time_buf = "%d" % (dead_time)
        self.motor_dead_time_line.setText(motor_dead_time_buf)
        self.actual_motor_dead_time_line.setText(actual_dead_times[dead_time])

    def stateChanged_spi(self, value):
        if (value == 2):
            self.motor_command_source_profile_check_box.setChecked(False)

    def stateChanged_profile(self, value):
        if (value == 2):
            self.motor_command_source_spi_check_box.setChecked(False)

    def stateChanged_sine_check(self, value):
        if (value == 2):
            #self.motor_commutation_hall_check_box.setCheckState
            self.motor_commutation_hall_check_box.setChecked(False)

            self.motor_commutation_sine_foc_check_box.setChecked(False)

            self.motor_commutation_hall_foc_check_box.setChecked(False)

    def stateChanged_hall_foc_check(self, value):
        if (value == 2):
            #self.motor_commutation_hall_check_box.setCheckState
            self.motor_commutation_hall_check_box.setChecked(False)

            self.motor_commutation_sine_foc_check_box.setChecked(False)

            self.motor_commutation_sine_check_box.setChecked(False)

    def stateChanged_hall_check(self, value):
        if (value == 2):
            #self.motor_commutation_hall_check_box.setCheckState
            self.motor_commutation_sine_check_box.setChecked(False)

            self.motor_commutation_sine_foc_check_box.setChecked(False)

            self.motor_commutation_hall_foc_check_box.setChecked(False)

    def stateChanged_sine_foc_check(self, value):
        if (value == 2):
            #self.motor_commutation_hall_check_box.setCheckState
            self.motor_commutation_hall_check_box.setChecked(False)

            self.motor_commutation_sine_check_box.setChecked(False)

            self.motor_commutation_hall_foc_check_box.setChecked(False)

    
   




    #def changeValue_dead_time_box(self, value):
        #dead_time = 25#(value * 100)/56
        #self.sld_dead_time.setTickPosition (self.sld_dead_time, dead_time)



    def __init__(self, parent=None):
        super(ThunderboxGUI, self).__init__(parent)

        try:
            if(ser):
                self.motor_status_label = QtGui.QLabel("Serial at /dev/ttyUSB0")
        except:
            self.motor_status_label = QtGui.QLabel("Serial open failed")

        self.motor_current_A_label = QtGui.QLabel("Current A: ")
        self.motor_current_B_label = QtGui.QLabel("Current B: ")
      
        self.get_current_button = QtGui.QPushButton("Get Current")
       

        self.event_status_label = QtGui.QLabel("Event Status: ")
        self.get_event_status_button = QtGui.QPushButton("Get Status")



        self.motor_vbus_label = QtGui.QLabel("Bus Voltage: ")
        self.temp_label = QtGui.QLabel("Temperature: ")

        
        motor_mode_label = QtGui.QLabel("Closed Loop")
        #self.motor_mode_line = QtGui.QLineEdit()
        self.motor_mode_check_box = QtGui.QCheckBox('On/Off', self) 

        motor_command_label = QtGui.QLabel("Motor Command")
        self.motor_command_line = QtGui.QLineEdit()

        motor_spi_command_label = QtGui.QLabel("SPI Command")
        self.motor_spi_command_line = QtGui.QLineEdit()

        motor_velocity_label = QtGui.QLabel("Motor Velocity (counts/cycle)")
        self.motor_velocity_line = QtGui.QLineEdit()

        motor_acceleration_label = QtGui.QLabel("Acceleration (Counts/Cycle)")
        self.motor_acceleration_line = QtGui.QLineEdit()

        motor_velocity_scalar_label = QtGui.QLabel("Velocity Scalar")
        self.motor_velocity_scalar_line = QtGui.QLineEdit()

        motor_velocity_loop_gain_kp_label = QtGui.QLabel("Velocity Gain Kp")
        self.motor_velocity_loop_gain_kp_line = QtGui.QLineEdit()

        motor_velocity_loop_gain_ki_label = QtGui.QLabel("Velocity Gain Ki")
        self.motor_velocity_loop_gain_ki_line = QtGui.QLineEdit()

        motor_velocity_loop_gain_limit_label = QtGui.QLabel("Velocity Gain Limit")
        self.motor_velocity_loop_gain_limit_line = QtGui.QLineEdit()

        motor_current_loop_gain_kp_label = QtGui.QLabel("Current Gain Kp")
        self.motor_current_loop_gain_kp_line = QtGui.QLineEdit()

        motor_current_loop_gain_ki_label = QtGui.QLabel("Current Gain Ki")
        self.motor_current_loop_gain_ki_line = QtGui.QLineEdit()

        motor_current_loop_gain_limit_label = QtGui.QLabel("Current Gain Limit")
        self.motor_current_loop_gain_limit_line = QtGui.QLineEdit()

	motor_temp_max_label = QtGui.QLabel("Max Temp (Degrees Celsius)")
        self.motor_temp_max_line = QtGui.QLineEdit()



        motor_commutation_mode_label = QtGui.QLabel("Commutation Mode")
        #self.motor_commutation_mode_line = QtGui.QLineEdit()
        self.motor_commutation_sine_check_box = QtGui.QCheckBox('Sine', self) 
        self.motor_commutation_sine_check_box.stateChanged[int].connect(self.stateChanged_sine_check)

        self.motor_commutation_hall_check_box = QtGui.QCheckBox('Trapezoidal', self) 
        self.motor_commutation_hall_check_box.setChecked(True)
 
        self.motor_commutation_hall_check_box.stateChanged[int].connect(self.stateChanged_hall_check)


        self.motor_commutation_sine_foc_check_box = QtGui.QCheckBox('Sine FOC', self) 

        self.motor_commutation_sine_foc_check_box.stateChanged[int].connect(self.stateChanged_sine_foc_check)


        self.motor_commutation_hall_foc_check_box = QtGui.QCheckBox('Trapezoidal FOC', self) 
        self.motor_commutation_hall_foc_check_box.stateChanged[int].connect(self.stateChanged_hall_foc_check)

        self.motor_command_source_label = QtGui.QLabel("Command Source")
        self.motor_command_source_spi_check_box = QtGui.QCheckBox('SPI', self)
        self.motor_command_source_spi_check_box.stateChanged[int].connect(self.stateChanged_spi)

        self.motor_command_source_profile_check_box = QtGui.QCheckBox('Profile Generator', self)
        self.motor_command_source_profile_check_box.stateChanged[int].connect(self.stateChanged_profile)

        self.loop_mode_current_check_box = QtGui.QCheckBox('Current', self)
        self.loop_mode_velocity_check_box = QtGui.QCheckBox('Velocity', self)
        self.loop_mode_velocity_integrate_check_box = QtGui.QCheckBox('Velocity Integrator', self)

        motor_loop_mode_label = QtGui.QLabel("Loop Mode")

        motor_limit_label = QtGui.QLabel("Motor Limit")
        self.motor_limit_line = QtGui.QLineEdit()

        motor_dead_time_label = QtGui.QLabel("Dead Time")
        self.motor_dead_time_line = QtGui.QLineEdit()
        actual_dead_time_label = QtGui.QLabel("Actual Dead Time:")
        self.actual_motor_dead_time_line = QtGui.QLineEdit()

        self.sld_dead_time = QtGui.QSlider(QtCore.Qt.Horizontal, self)
        self.sld_dead_time.setFocusPolicy(QtCore.Qt.NoFocus)
        self.sld_dead_time.valueChanged[int].connect(self.changeValue_dead_time)


       # self.motor_dead_time_line.textChanged[str].connect(self.changeValue_dead_time_box)

        motor_phase_counts_label= QtGui.QLabel("Phase Counts")
        self.motor_phase_counts_line= QtGui.QLineEdit()

        self.write_button = QtGui.QPushButton("Write")
        self.write_button.show()
        self.read_button = QtGui.QPushButton("Read")
        self.read_button.show()
	self.close_button = QtGui.QPushButton("Close")
	self.close_button.show()
        self.enable_button = QtGui.QPushButton("Enable")
        self.enable_button.show()
        self.look_out_button = QtGui.QPushButton("Enable Auto-Update")
        self.look_out_button.show()

        self.hash_button = QtGui.QPushButton("Recompute Base Hash Key")
        self.hash_button.show()

        self.command_button = QtGui.QPushButton("Set")
        self.command_button.show()
        self.get_command_button = QtGui.QPushButton("Get")
        self.get_command_button.show()

        self.spi_command_button = QtGui.QPushButton("Set")
        self.get_spi_command_button = QtGui.QPushButton("Get")

        self.velocity_button = QtGui.QPushButton("Vset")
        self.velocity_button.show()
        self.get_velocity_button = QtGui.QPushButton("Vget")
        self.get_velocity_button.show()

        self.update_button = QtGui.QPushButton("Update")
        self.update_button.show()

        self.estop_button = QtGui.QPushButton("ESTOP")
        self.estop_button.show()


        self.current_A_error_label = QtGui.QLabel("Current A Error")
        self.current_B_error_label = QtGui.QLabel("Current B Error")


        self.get_error_button = QtGui.QPushButton("Get Error")
        self.get_error_button.show()
        self.set_offset_button = QtGui.QPushButton("Set Offset")
        self.set_offset_button.show()



        self.write_button.clicked.connect(self.send_data)
        self.read_button.clicked.connect(self.read_data)
        self.enable_button.clicked.connect(self.enable)
        self.look_out_button.clicked.connect(self.look_out)
        self.command_button.clicked.connect(self.command)
        self.get_command_button.clicked.connect(self.get_command)

        self.spi_command_button.clicked.connect(self.spi_command)
        self.get_spi_command_button.clicked.connect(self.get_spi_command)

        self.velocity_button.clicked.connect(self.velocity)
        self.get_velocity_button.clicked.connect(self.get_velocity)
        self.update_button.clicked.connect(self.demand_acknowledge)
        self.estop_button.clicked.connect(self.emergency_stop)

        self.get_current_button.clicked.connect(self.get_current)

        self.get_event_status_button.clicked.connect(self.get_event_status)

        self.get_error_button.clicked.connect(self.get_error)
        self.set_offset_button.clicked.connect(self.set_offset)







        mainLayout = QtGui.QGridLayout()


        mainLayout.addWidget(self.motor_status_label, 0, 0)
        mainLayout.addWidget(self.update_button, 0, 5)
        mainLayout.addWidget(self.motor_vbus_label, 0, 6)
        mainLayout.addWidget(self.temp_label, 1, 6)
        mainLayout.addWidget(self.motor_current_A_label, 2, 6)
        mainLayout.addWidget(self.get_current_button, 2, 5)
        mainLayout.addWidget(self.motor_current_B_label, 3, 6)
        mainLayout.addWidget(self.set_offset_button, 3, 5)
       

        mainLayout.addWidget(self.current_A_error_label, 4, 6)
        mainLayout.addWidget(self.current_B_error_label, 5, 6)
        mainLayout.addWidget(self.get_error_button, 4, 5)
       

        mainLayout.addWidget(self.event_status_label, 6, 6)
        mainLayout.addWidget(self.get_event_status_button, 6, 5);




        mainLayout.addWidget(motor_commutation_mode_label, 1, 0)
        mainLayout.addWidget(self.motor_commutation_sine_check_box, 1, 1)
        mainLayout.addWidget(self.motor_commutation_hall_check_box, 1, 2)
        mainLayout.addWidget(self.motor_commutation_sine_foc_check_box, 1, 3)
        mainLayout.addWidget(self.motor_commutation_hall_foc_check_box, 1, 4)

        mainLayout.addWidget(self.motor_command_source_label, 2, 0)
        mainLayout.addWidget(self.motor_command_source_spi_check_box, 2, 1)
        mainLayout.addWidget(self.motor_command_source_profile_check_box, 2, 2)
 
        mainLayout.addWidget(motor_mode_label, 3, 0)
        mainLayout.addWidget(self.motor_mode_check_box, 3, 1)

 
        mainLayout.addWidget(motor_loop_mode_label, 4, 0)
        mainLayout.addWidget(self.loop_mode_current_check_box, 4, 1)
        mainLayout.addWidget(self.loop_mode_velocity_check_box, 4, 2)
        mainLayout.addWidget(self.loop_mode_velocity_integrate_check_box, 4, 3)

      
        mainLayout.addWidget(motor_dead_time_label, 5, 0)
        mainLayout.addWidget(self.motor_dead_time_line, 5, 1)
        mainLayout.addWidget(self.sld_dead_time, 5, 2)
        mainLayout.addWidget(actual_dead_time_label, 5, 3)
        mainLayout.addWidget(self.actual_motor_dead_time_line, 5, 4)

        mainLayout.addWidget(motor_phase_counts_label, 6, 0)
        mainLayout.addWidget(self.motor_phase_counts_line, 6, 1)

        mainLayout.addWidget(motor_velocity_scalar_label, 7, 0)
        mainLayout.addWidget(self.motor_velocity_scalar_line, 7, 1)

        mainLayout.addWidget(motor_acceleration_label, 8, 0)
        mainLayout.addWidget(self.motor_acceleration_line, 8, 1)

        mainLayout.addWidget(motor_velocity_loop_gain_kp_label, 9, 0)
        mainLayout.addWidget(self.motor_velocity_loop_gain_kp_line, 9, 1)

        mainLayout.addWidget(motor_velocity_loop_gain_ki_label, 10, 0)
        mainLayout.addWidget(self.motor_velocity_loop_gain_ki_line, 10, 1)

        mainLayout.addWidget(motor_velocity_loop_gain_limit_label, 11, 0)
        mainLayout.addWidget(self.motor_velocity_loop_gain_limit_line, 11, 1)


        mainLayout.addWidget(motor_current_loop_gain_kp_label, 12, 0)
        mainLayout.addWidget(self.motor_current_loop_gain_kp_line, 12, 1)

        mainLayout.addWidget(motor_current_loop_gain_ki_label, 13, 0)
        mainLayout.addWidget(self.motor_current_loop_gain_ki_line, 13, 1)

        mainLayout.addWidget(motor_current_loop_gain_limit_label, 14, 0)
        mainLayout.addWidget(self.motor_current_loop_gain_limit_line, 14, 1)

	mainLayout.addWidget(motor_temp_max_label, 15, 0)
        mainLayout.addWidget(self.motor_temp_max_line, 15, 1)

        mainLayout.addWidget(motor_limit_label, 16, 0)
        mainLayout.addWidget(self.motor_limit_line, 16, 1)

        mainLayout.addWidget(motor_command_label, 17, 0)
        mainLayout.addWidget(self.motor_command_line, 17, 1)
        mainLayout.addWidget(self.command_button, 17, 2)
        mainLayout.addWidget(self.get_command_button, 17, 3)

        mainLayout.addWidget(motor_velocity_label, 18, 0)
        mainLayout.addWidget(self.motor_velocity_line, 18, 1)
        mainLayout.addWidget(self.velocity_button, 18, 2)
        mainLayout.addWidget(self.get_velocity_button, 18, 3)

        mainLayout.addWidget(motor_spi_command_label, 19,0)
        mainLayout.addWidget(self.motor_spi_command_line, 19,1)
        mainLayout.addWidget(self.spi_command_button, 19,2)
        mainLayout.addWidget(self.get_spi_command_button, 19,3)
 
       
        mainLayout.addWidget(self.write_button, 20, 0)
        mainLayout.addWidget(self.read_button, 20, 1)
	mainLayout.addWidget(self.close_button, 20, 2)
        mainLayout.addWidget(self.enable_button, 20,3)
        mainLayout.addWidget(self.look_out_button, 20,4)
        mainLayout.addWidget(self.estop_button, 20, 5)
        mainLayout.addWidget(self.hash_button, 20, 6)



        self.setLayout(mainLayout)
        self.setWindowTitle("THUNDERBOX GUI")

if __name__ == '__main__':
    import sys
    app = QtGui.QApplication(sys.argv)

    thunderbox_gui = ThunderboxGUI()
    thunderbox_gui.show()

    sys.exit(app.exec_())
